// Copyright (c) 2022 ChenJun
// Licensed under the Apache-2.0 License.

#ifndef RM_SERIAL_DRIVER__PACKET_HPP_
#define RM_SERIAL_DRIVER__PACKET_HPP_

#include <algorithm>
#include <cstdint>
#include <vector>

namespace rm_serial_driver
{

  // 报文类型定义
  enum PacketTypeEnum
  {
    ENUM_PACKET_HEART_BEAT = 0,
    ENUM_PACKET_SENTRY_ACCEPT_STATUS_DATA = 1,
    ENUM_PACKET_ARMOR_DATA = 2,
    ENUM_PACKET_SENTRY_SEND_CONTROL_DATA, // 哨兵--装甲板信息
    ENUM_PACKET_GIMBAL_RESET,             // 云台复位
    ENUM_PACKET_RADAR_SMALL_MAP,          // 雷达发送的消息
    ENUM_PACKET_OBSTACLE_DATE,            // 障碍快信息
    ENUM_PACKET_UNDEFINED,
    ENUM_PACKET_INFANTRY_ACCEPT_STATUS_DATA,
    ENUM_PACKET_BUFF_DATA,
    ENUM_PACKET_LOCATION,
    ENUM_PACKET_CMDCONTROL,
    ENUM_PACKET_DETECTOR_DATA,
    ENUM_PACKET_GAMESTATUS_DATA,
    ENUM_PACKET_NAV_DATA
  };

  enum ArmEnum
  {
    ENUM_ARM_SLAVE_COMPUTER = 0, // 下位机，指电控
    ENUM_ARM_HERO = 1,
    ENUM_ARM_MINER = 2,
    ENUM_ARM_INFANTRY3 = 3, // 3号步兵
    ENUM_ARM_INFANTRY4 = 4, // 4号步兵
    ENUM_ARM_INFANTRY5 = 5, // 5号步兵
    ENUM_ARM_UAV,
    ENUM_ARM_SENTRY,
    ENUM_ARM_DARTS,    // 飞镖
    ENUM_ARM_RADAR,    // 雷达
    ENUM_ARM_OPERATOR, // 操作手
    ENUM_ARM_BASE,
    ENUM_ARM_OUTPOST,
    ENUM_ARM_UNDEFINED, // 未定义兵种
    ENUM_ARM_ALL = 0xff
  };

  struct PacketHeader
  {
    uint8_t start1 = 0xa5;
    uint8_t start2 = 0x5a;
    uint8_t from = 0;
    uint8_t to = 0;
    uint8_t packet_type = 0;
    uint8_t data_len = 0; // 数据字段长度
    uint16_t checksum = 0;
  } __attribute__((packed));

  typedef struct _NavAcceptStatusData
  {
    float x;   //-y
    float y;   //-z
    float z;   //-z
    float yaw; // x
    uint8_t is_reach;

    _NavAcceptStatusData(float _x, float _y,float _z, float _yaw, uint8_t _is_reach)
        : x(_x), y(_y), z(_z), yaw(_yaw), is_reach(_is_reach)
    {
    }

  } __attribute__((packed)) NavAcceptStatusData;

  typedef struct _SendPacketNav
  {
    // PacketHeader header;
    uint8_t start1 = 0xa5;
    uint8_t start2 = 0x5a;
    uint8_t from = 0;
    uint8_t to = 0;
    uint8_t packet_type = 0;
    uint8_t data_len = 0; // 数据字段长度
    uint16_t checksum = 0;

    float vx_mps; /* x轴，即前进速度，单位 m/s */
    float vy_mps; /* y轴，即向左速度，单位 m/s */
    float vw_rpm; /* z轴，即旋转速度，单位 */
    float current_x;
    float current_y;   // 单位m
    float current_yaw; // 与x轴夹角，单位rad
    float exp_yaw;
    // uint16_t checksum = 0;

    _SendPacketNav(float _vx_mps, float _vy_mps, float _vw_rpm, float _current_x, 
                    float _current_y, float _current_yaw, float _exp_yaw)
                    : vx_mps(_vx_mps), vy_mps(_vy_mps), vw_rpm(_vw_rpm), current_x(_current_x), 
                    current_y(_current_y), current_yaw(_current_yaw), exp_yaw(_exp_yaw)
    {
    }

  } __attribute__((packed)) SendPacketNav;

  template <typename T>
  inline T fromVector(const std::vector<uint8_t> &data)
  {
    T packet;
    std::copy(data.begin(), data.end(), reinterpret_cast<uint8_t *>(&packet));
    return packet;
  }

  template <typename T>
  inline std::vector<uint8_t> toVector(const T &data)
  {
    std::vector<uint8_t> packet(sizeof(T));
    std::copy(
        reinterpret_cast<const uint8_t *>(&data), reinterpret_cast<const uint8_t *>(&data) + sizeof(T),
        packet.begin());
    return packet;
  }

  static inline uint16_t calChecksum(const char *__data, const size_t __len)
  {
    uint16_t my_checksum = 0;
    const char *ptr = __data;
    for (int i = 0; i < (int)(__len / 2); i++)
    {
      my_checksum += *((uint16_t *)ptr);
      ptr += 2;
    }
    if (__len & 0x01)
    {
      uint16_t tmp = uint16_t(__data[__len - 1]);
      my_checksum += tmp;
    }

    return my_checksum;
  }
} // namespace rm_serial_driver

#endif // RM_SERIAL_DRIVER__PACKET_HPP_
